package oceanlife.pathfinding;

import java.awt.geom.Line2D;
import java.util.ArrayList;
import java.util.Iterator;

public class Line2 extends Line2D.Double implements Iterable<Pnt> {
	/**
	 * 
	 */
	private double eps = 1e-10;
	private static final long serialVersionUID = 1L;
	private Pnt startPnt;
	private Pnt endPnt;
	
	public Line2(Pnt p1, Pnt p2){
		super(p1.coord(0),p1.coord(1),p2.coord(0),p2.coord(1));
		startPnt = p1;
		endPnt = p2;
	}
	
	public Line2(double x1,double y1,double x2,double y2){
		super(x1,y1,x2,y2);
		this.startPnt = new Pnt(x1,y1);
		this.endPnt = new Pnt(x2,y2);
	}
	
	public Pnt getStart(){
		return startPnt;
	}
	
	
	public Pnt getEnd(){
		return endPnt;
	}
	
	public Pnt[] findCircleIntersection(Pnt centre, double radius) {
		Pnt d = endPnt.subtract(startPnt); 
		Pnt f = startPnt.subtract(centre);
		double a = d.dot(d) ;
		double b = 2*f.dot(d) ;
		double c = f.dot(f) - radius*radius ;
		Pnt sol1 = null;
		Pnt sol2;

		double discriminant = b*b-4*a*c;
		if (discriminant < 0){
		  return new Pnt[]{null,null};
		} else {
		  discriminant = Math.sqrt(discriminant);
		  double t1 = (-b + discriminant)/(2*a);
		  double t2 = (-b - discriminant)/(2*a);
		  if( t1 >= 0 && t1 <= 1 ){
			  sol1 = new Pnt(startPnt.coord(0)+t1*d.coord(0),startPnt.coord(1)+t1*d.coord(1));
			  sol1 = checkOnLine(startPnt,endPnt,sol1);
		  }else{
		    
		  }
		  sol2 = new Pnt(startPnt.coord(0)+t2*d.coord(0),startPnt.coord(1)+t2*d.coord(1));
		  sol2 = checkOnLine(startPnt,endPnt,sol2);
		}
		return new Pnt[]{sol1,sol2};
	}

	private Pnt checkOnLine(Pnt linePnt1, Pnt linePnt2, Pnt sol) {
		  for(int i = 0; i < 2;i++){
			  if(linePnt1.coord(i) > linePnt2.coord(i)){
				  if(linePnt2.coord(i) > sol.coord(i) || sol.coord(i) > linePnt1.coord(i)){
					  sol = null;
					  break;
				  }
			  } else {
				  if(linePnt2.coord(i) < sol.coord(i) || sol.coord(i) < linePnt1.coord(i)){
					  sol = null;
					  break;
				  }
			  }
		  }
		  return sol;
	}

	public double angle() {
		Pnt vector = startPnt.subtract(endPnt);
		//double angle = startPnt.angle(endPnt);
		double angle = Math.atan2(vector.coord(0),vector.coord(1));
		//if(vector.coord(1)<0){
		//	angle = Math.PI-angle;
		//}
		//if(vector.coord(0)<0){
		//	angle += Math.PI;
		//}
		
		//System.out.println(vector.coord(0)+" "+vector.coord(1)+" "+angle);
		
		
		return angle;
	}
	
	//return shorts line connecting the two lines. null if intersection.
	public Line2 getShortestLineTo(Line2 other){
		if(this.intersectionPoint(other) != null){
			return null;
		} else {
			Pnt thisPnt = this.startPnt;
			Pnt otherPnt = other.startPnt;
			Pnt temp = this.closestPoint(other.startPnt);
			if(temp.distanceTo(otherPnt) < thisPnt.distanceTo(otherPnt)){
				thisPnt = temp;
			}
			temp = this.closestPoint(other.endPnt);
			if(temp.distanceTo(otherPnt) < thisPnt.distanceTo(otherPnt)){
				thisPnt = temp;
			}
			temp = other.closestPoint(this.startPnt);
			if(thisPnt.distanceTo(temp) < thisPnt.distanceTo(otherPnt)){
				otherPnt = temp;
			}
			temp = other.closestPoint(this.endPnt);
			if(thisPnt.distanceTo(temp) < thisPnt.distanceTo(otherPnt)){
				otherPnt = temp;
			}
			return new Line2(thisPnt,otherPnt);
		}
	}
	
	//returns closest point on line in relation to pnt
	public Pnt closestPoint(Pnt pnt){
		final double xDelta = endPnt.coord(0) - startPnt.coord(0);
		final double yDelta = endPnt.coord(1) - startPnt.coord(1);

		if ((xDelta == 0) && (yDelta == 0)) {
		    return null;
		}

		final double u = ((pnt.coord(0) - startPnt.coord(0)) * xDelta + (pnt.coord(1) - startPnt.coord(1)) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

		Pnt closestPoint;
		if (u < 0) {
		    closestPoint = startPnt;
		} else if (u > 1) {
		    closestPoint = endPnt;
		} else {
		    closestPoint = new Pnt(startPnt.coord(0) + u * xDelta, startPnt.coord(1) + u * yDelta);
		}

		return closestPoint;
	}
	
	public double length(){
		return startPnt.distanceTo(endPnt);
	}
	
	public Pnt[] getPnts(){
		return new Pnt[]{startPnt,endPnt};
	}
	
	@Override
	public boolean equals(Object other){
        if (!(other instanceof Line2)) return false;
        Line2 line = (Line2) other;
		if(this.endPnt.equals(line.endPnt) && this.startPnt.equals(line.startPnt)){
			return true;
		}
		if(this.endPnt.equals(line.startPnt) && this.startPnt.equals(line.endPnt)){
			return true;
		}
		return false;
	}
	
    public Iterator<Pnt> iterator () {
    	final ArrayList<Pnt> pnts = new ArrayList<Pnt>();
    	pnts.add(startPnt);
    	pnts.add(endPnt);
        return new Iterator<Pnt>() {
            private Iterator<Pnt> it = pnts.iterator();
            public boolean hasNext() {return it.hasNext();}
            public Pnt next() {return it.next();}
            public void remove() {throw new UnsupportedOperationException();}
        };
    }
    public Pnt intersectionPoint(Line2 line){
    	//TODO epsilon goed laten werken
    	
		double x1 = line.getStart().coord(0), y1 = line.getStart().coord(1), x2 = line.getEnd().coord(0), y2 = line.getEnd().coord(1);
		double x3 = this.getStart().coord(0), y3 = this.getStart().coord(1), x4 = this.getEnd().coord(0), y4 = this.getEnd().coord(1);
		double d = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4);
		if (d == 0.0) return null;
		double xi = ((x3-x4)*(x1*y2-y1*x2)-(x1-x2)*(x3*y4-y3*x4))/d;
		double yi = ((y3-y4)*(x1*y2-y1*x2)-(y1-y2)*(x3*y4-y3*x4))/d;
		
		//check if pnt is between linepoints
		double minXa = Math.min(x1, x2), minXb = Math.min(x3, x4);
		double minYa = Math.min(y1, y2), minYb = Math.min(y3, y4);
		double maxXa = Math.max(x1, x2), maxXb = Math.max(x3, x4);
		double maxYa = Math.max(y1, y2), maxYb = Math.max(y3, y4);
		if(
				(minXa-eps <= xi && xi <= maxXa+eps && minYa-eps <= yi && yi <= maxYa+eps)&&
				(minXb-eps <= xi && xi <= maxXb+eps && minYb-eps <= yi && yi <= maxYb+eps)
			){
			return new Pnt(xi,yi);
		} else {
			return null;
		}
    }
    
    public Pnt getMiddlePnt(){
    	return new Pnt((startPnt.coord[0]+endPnt.coord[0])/2,(startPnt.coord[1]+endPnt.coord[1])/2);
    }
    
    @Override
    public String toString(){
    	return "Line("+startPnt+","+endPnt+")";
    }
    
    public static void main(String[] args) {
		Line2 lineA = new Line2(new Pnt(-1.0,0.0),new Pnt(0.0,1.0));
		Line2 lineB = new Line2(new Pnt(1.0,-1.0),new Pnt(1.0,8.0));
		System.out.println(lineA.intersectionPoint(lineB));
		
		Line2 lineC = new Line2(new Pnt(0,0),new Pnt(-1,0));
		System.out.println(lineC.angle());
	}

	public boolean pointOnLine(Pnt inter) {
		double ratio1 = (startPnt.coord(0)-inter.coord(0))/(startPnt.coord(0)-endPnt.coord(0));
		double ratio2 = (startPnt.coord(1)-inter.coord(1))/(startPnt.coord(1)-endPnt.coord(1));
		if(Math.abs(ratio1 - ratio2) < eps){
			if(startPnt.coord(0) < inter.coord(0) && (inter.coord(0) < endPnt.coord(0))){
				if(startPnt.coord(1) < inter.coord(1) && (inter.coord(1) < endPnt.coord(1))){
					return true;
				}
			} else if(startPnt.coord(0) > inter.coord(0) && (inter.coord(0) > endPnt.coord(0))){
				if(startPnt.coord(1) > inter.coord(1) && (inter.coord(1) > endPnt.coord(1))){
					return true;
				}
			}
			return true;
		}
		return false;
	}
    
}
